//
// Created by ze on 2023/12/30.
//
#include "UPRE_GLOBAL_DEFINE.h"
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_PATH.h"
#include "UPRE_PID.h"
#include "UPRE_LOCATION.h"
#include "UPRE_GYRO.h"
#include "tim.h"
#include "UPRE_Decision.h"
#include "can.h"
#include "Kalmanfilter.h"

//
// Created by ze on 2023/11/10.
//
uint8_t g_Auto_Num = 0;
Chassis_Sense_Bool_Def Chassis_Sense_Bool = {false};
Chassis_Sense_Value_Def Chassis_Sense_Value = {
        .Last_Which_Goal = 0,
        . High_To_Goal_Top = 625.f,
        .High_To_Goal_Bottom = 0.f,
        .Which_Goal = 0,
        .DT35_Fix = 0.f,
        .Store_Fix_X = 0.f,
};
//重试标志位
bool Is_Remake = false;
uint8_t Num_To_Goal = 0;
uint8_t Flag_2_ICE = 0x05;
uint8_t USART1_RX[100] = {0};
uint8_t gyro_sum = 0;
point Camera_Return = {0};
static PATH_BesselControlPoint Bessel_Point = {0};

//直线路径任务
static StraightLinePath line = { 0 };
//static Pose gopath2 = { 5000, 5000, 0 };  //直线路径1
int32_t Time_ = 0;

//返回谷仓位置
static Pose Return_Goal(void)
{
    static Pose Line_ = {0};
    if(Chassis_Sense_Value.Which_Goal != 0) {
        Line_.yaw = -92.67f;
        if (Chassis_Sense_Value.Which_Goal == 1) {
            Line_.wx = Goal1_x;
            Line_.wy = Goal1_y;

        }
        else if (Chassis_Sense_Value.Which_Goal == 2) {
            Line_.wx = Goal2_x;
            Line_.wy = Goal2_y;

        }
        else if (Chassis_Sense_Value.Which_Goal == 3) {
            Line_.wx = Goal3_x;
            Line_.wy = Goal3_y;

        }
        else if (Chassis_Sense_Value.Which_Goal == 4) {
            Line_.wx = Goal4_x;
            Line_.wy = Goal4_y;

        }
        else if (Chassis_Sense_Value.Which_Goal == 5) {
            Line_.wx = Goal5_x;
            Line_.wy = Goal5_y;
        }
        Chassis_Sense_Value.Last_Which_Goal = Chassis_Sense_Value.Which_Goal;
    }
    return Line_;
}
/*******************************************************************************/
//TODO//相机调整路径
//后退路径
static uint8_t Adjust_Camera_Num = 0;
static bool Adjust_Camera_Path(void)
{
    bool result = false;

    static Pose line2 = {0};
    switch (Adjust_Camera_Num)
    {
        case 0:{

            line2.yaw = -92.67f;
            line2.wx = 9379.f;
            line2.wy = 4500.f;
            Bessel_Point.Path_BesselPoint_1 = Robot_Pose;
            Bessel_Point.Path_BesselPoint_2.x = line2.wx;
            Bessel_Point.Path_BesselPoint_2.y = Robot_Pose.wy-100.f;
            Bessel_Point.Path_BesselPoint_3.x = line2.wx;
            Bessel_Point.Path_BesselPoint_3.y = Robot_Pose.wy-100.f;
            Bessel_Point.Path_BesselPoint_4 = line2;
            Adjust_Camera_Num++;
        }
            break;
        case 1:
            if(Path_BesselPathInit(&Bessel_Point,false,600,600,0,
                                   10,20,10))
                Adjust_Camera_Num++;
            break;
        case 2:
            if (goPath() == FINISH)
            {
                result = true;
                Adjust_Camera_Num++;
            }
            break;
        default:
            break;
    }
    return result;
}
/*******************************************************************************/
//正常启动路径
/*******************************************************************************/
void PATH_GoB_SplinePAth(void)
{
    static Pose StraightPath = {0};
    static float Last_DT35 = 0.f;
    static bool Once_Fix = true;
    static BsplinePath path_BsplinePath = {0};

    switch (g_Auto_Num) {
        case 0:                 //B样条曲线开启
            Flag_2_ICE = 0x05;
            path_BsplinePath.ctrlPoint1 = Robot_Pose;

            path_BsplinePath.ctrlPoint2.x = 10461.f;
            path_BsplinePath.ctrlPoint2.y = -907.f;

            path_BsplinePath.ctrlPoint3.x = 2553.f;
            path_BsplinePath.ctrlPoint3.y = 4750.f;

//            path_BsplinePath.ctrlPoint4.wx = 7908.89f;
            path_BsplinePath.ctrlPoint4.wx = 7988.89f;
            path_BsplinePath.ctrlPoint4.wy = 3650.79f;

            path_BsplinePath.ctrlPoint4.yaw = 0.f;

            g_Auto_Num++;
            if(Is_Remake)
                g_Auto_Num = 3;
            break;
        case 1:
            if(PATH_B_SpLinePathInit_4ControlPoint(&path_BsplinePath, false, 700, 800, 800,
                                                   0, 0, 10))
                g_Auto_Num++;
            break;
        case 2:                     //已上坡
            if (goPath() == FINISH) {
                g_Auto_Num++;
            }
            break;
        case 3:                     //开始移动到视觉的识别点
            StraightPath.wx = 9379.f;
            StraightPath.wy = 3812.f;
            StraightPath.yaw = -92.67f;
            Bessel_Point.Path_BesselPoint_1 = Robot_Pose;
            Bessel_Point.Path_BesselPoint_2.x = (StraightPath.wx - Robot_Pose.wx) * 0.3f + Robot_Pose.wx;
            Bessel_Point.Path_BesselPoint_2.y = (StraightPath.wy - Robot_Pose.wy) * 0.3f + Robot_Pose.wy;
            Bessel_Point.Path_BesselPoint_3.x = (StraightPath.wx - Robot_Pose.wx) * 0.6f + Robot_Pose.wx;
            Bessel_Point.Path_BesselPoint_3.y = (StraightPath.wy - Robot_Pose.wy) * 0.6f + Robot_Pose.wy;
            Bessel_Point.Path_BesselPoint_4 = StraightPath;
            g_Auto_Num++;
            break;
        case 4:
            if(Path_BesselPathInit(&Bessel_Point,false,600,600,200,
                                   10,30,85))
                g_Auto_Num++;
            break;
        case 5:                //到达视觉识别点
            Chassis_Sense_Bool.Flag_ReachZone3 = true;
            Flag_2_ICE = 0x03;
            if (goPath() == FINISH) {
                Percept_RePack.DivideBall.is_find_ball=true;
                g_Auto_Num++;
            }
            break;
        case 6:                 //给相机发消息开始识别
            Flag_2_ICE = 0x03;
            if(Chassis_Sense_Value.Last_Which_Goal == 1 || Chassis_Sense_Value.Last_Which_Goal == 5)
            {
                if(Adjust_Camera_Path())
                {
                    Adjust_Camera_Num = 0;
                    Chassis_Sense_Value.Last_Which_Goal = 0;
                }

            }
            Chassis_Sense_Value.Which_Goal = 0;
            Chassis_Sense_Bool.Recv_Blue_Ball = false;
            if(Chassis_Sense_Bool.Start_Find_Ball)              //直到收到相机发送坐标才停止
            {
                Flag_2_ICE = 0x05;
                Chassis_Sense_Bool.Start_Find_Ball = false;
                g_Auto_Num++;
            }
            break;
        case 7:                 //接收到相机的坐标  //TODO//找球
            Camera_Return.x = Percept_RePack.TargetBall.Target_ball_x;
            Camera_Return.y = Percept_RePack.TargetBall.Target_ball_y;
            Camera_Return.x = Limit_Max(Camera_Return.x,10900);
            Camera_Return.x = Limit_Min(Camera_Return.x,7722);
            StraightPath.yaw = -92.67f;
            StraightPath.wx = Camera_Return.x;
            StraightPath.wy = 235.f;
            Bessel_Point.Path_BesselPoint_1 = Robot_Pose;
            Bessel_Point.Path_BesselPoint_2.x = StraightPath.wx;

            if(Num_To_Goal)
                Bessel_Point.Path_BesselPoint_2.y = Robot_Pose.wy - 500;
            else
                Bessel_Point.Path_BesselPoint_2.y = Robot_Pose.wy;

            Bessel_Point.Path_BesselPoint_3.x = StraightPath.wx;

            if(Num_To_Goal)
                Bessel_Point.Path_BesselPoint_3.y = Robot_Pose.wy - 500;
            else
                Bessel_Point.Path_BesselPoint_3.y = Robot_Pose.wy;

            Bessel_Point.Path_BesselPoint_4 = StraightPath;

            g_Auto_Num++;
            break;
        case 8:             //TODO//找球
            if(Path_BesselPathInit(&Bessel_Point,false,800,800,300,
                                   5,20,5)){
                g_Auto_Num++;
            }
            break;
        case 9:                 //如果找到球，强制关闭路径,如果没找到,就返回g_Auto_num 6
            if(Chassis_Sense_Bool.Recv_Blue_Ball == true || Percept_RePack.DivideBall.is_send_ball == true)
            {
                //分完球后给的标志位
                Chassis_Sense_Value.Last_Which_Goal = 0;
                Chassis_Sense_Value.Which_Goal = 0;
                g_Auto_Num += 4;
            }
            else if(goPath() == FINISH){//没找到球,重新找球
                g_Auto_Num++;
            }
            break;
        case 10:                    //TODO//后退路径
        if(Chassis_Sense_Bool.Robot_In_East_Half)
            StraightPath.wx = Robot_Pose.wx - 200.f;
        else
            StraightPath.wx = Robot_Pose.wx + 200.f;
            StraightPath.wy = Robot_Pose.wy + 1500.f;
            StraightPath.yaw = -92.67f;
            Bessel_Point.Path_BesselPoint_1 = Robot_Pose;
            Bessel_Point.Path_BesselPoint_2.x = (StraightPath.wx - Robot_Pose.wx) * 0.3f + Robot_Pose.wx;
            Bessel_Point.Path_BesselPoint_2.y = (StraightPath.wy - Robot_Pose.wy) * 0.3f + Robot_Pose.wy;
            Bessel_Point.Path_BesselPoint_3.x = (StraightPath.wx - Robot_Pose.wx) * 0.6f + Robot_Pose.wx;
            Bessel_Point.Path_BesselPoint_3.y = (StraightPath.wy - Robot_Pose.wy) * 0.6f + Robot_Pose.wy;
            Bessel_Point.Path_BesselPoint_4 = StraightPath;
            g_Auto_Num++;
            break;
        case 11:
            if(Path_BesselPathInit(&Bessel_Point,false,800,800,300,
                                   5,20,5))
                g_Auto_Num++;
            break;
        case 12:
            if (goPath() == FINISH){
                g_Auto_Num = 6;
            }
            break;
        case 13:                    //TODO//DT35校准点
            StraightPath.wx = 9379.f;
            StraightPath.wy = 3812.f;
            StraightPath.yaw = -92.67f;
            Bessel_Point.Path_BesselPoint_1 = Robot_Pose;
            Bessel_Point.Path_BesselPoint_2.x =Robot_Pose.wx;
            Bessel_Point.Path_BesselPoint_2.y = StraightPath.wy - 1300;
            Bessel_Point.Path_BesselPoint_3.x = Robot_Pose.wx ;
            Bessel_Point.Path_BesselPoint_3.y = StraightPath.wy - 1300;
            Bessel_Point.Path_BesselPoint_4 = StraightPath;
            g_Auto_Num++;
            Chassis_Sense_Bool.On_Time = true;
            break;
        case 14:
            if(Path_BesselPathInit(&Bessel_Point,false,800,800,0,
                                   10,30,10))
                g_Auto_Num++;
            break;
        case 15:
            if(goPath() == FINISH)
            {
                static bool Once = true;
                if(Chassis_Sense_Bool.On_Time)
                {
                    Time_ = 0;
                    Chassis_Sense_Bool.On_Time = false;
                }
                if (Once && Time_ >= 1000)
                {
                    if(DT35_distance[0]>1600.f && DT35_distance[0]<1950.f && fabsf(DT35_distance[0] - Last_DT35) <= 2.f)
                    {
                        Chassis_Sense_Value.Store_Fix_X = Robot_Pose.wx;
                        Chassis_Sense_Value.DT35_Fix_First = DT35_distance[0];          //只存储第一次修正值
                        Chassis_Sense_Bool.Start_DT35_Fix = true;
                        Once = false;
                        g_Auto_Num++;
                    }
                }
                else if (Time_>=800)
                {
                    if(DT35_distance[0]>1600.f && DT35_distance[0]<1950.f && fabsf(DT35_distance[0] - Last_DT35) <= 2.f)
                    {
                        Chassis_Sense_Value.DT35_Fix = DT35_distance[0] - Chassis_Sense_Value.DT35_Fix_First;
                        g_Auto_Num++;
                    }
                }
                Last_DT35 = DT35_distance[0];
            }
            break;
        case 16:
            //TODO//获取目标谷仓
            if(Chassis_Sense_Value.Which_Goal != 0)
            {
                StraightPath = Return_Goal();                    //TODO//L型归仓
                Bessel_Point.Path_BesselPoint_1 = Robot_Pose;
                Bessel_Point.Path_BesselPoint_2.x =StraightPath.wx;
                Bessel_Point.Path_BesselPoint_2.y = Robot_Pose.wy;
                Bessel_Point.Path_BesselPoint_3.x = StraightPath.wx ;
                Bessel_Point.Path_BesselPoint_3.y = Robot_Pose.wy;
                Bessel_Point.Path_BesselPoint_4 = StraightPath;
                g_Auto_Num++;
            }
            break;
        case 17:
            if(Path_BesselPathInit(&Bessel_Point,false,700,700,200,
                                   10,50,10)){
                g_Auto_Num++;
            }
            break;
        case 18:
            if(Chassis_Sense_Value.Last_Which_Goal != Chassis_Sense_Value.Which_Goal && (StraightPath.wy - Robot_Pose.wy) >= 800.f)
                g_Auto_Num-=2;
            else if(goPath() == FINISH)//到达谷仓,给电机发标志位,开始放球       //TODO
            {
                Timer_Stamp.TimeStamp_is_lay_ball=0;
                Percept_RePack.DivideBall.is_lay_ball = true;
                Percept_RePack.DivideBall.is_send_ball=false;
                Chassis_Sense_Value.Which_Goal = 0;
                g_Auto_Num++;
            }
            break;
        case 19:
            Flag_2_ICE = 0x03;
            if(Percept_RePack.DivideBall.is_find_ball && Chassis_Sense_Bool.Start_Find_Ball)
            {
                Chassis_Sense_Bool.GO_Target_Yaw = false;
                Chassis_Sense_Bool.Start_Find_Ball = false;
                Num_To_Goal++;
                g_Auto_Num = 6;//TODO
            }
            break;
    }

}
/*******************************************************************************/
//重试区路径
/*******************************************************************************/
uint8_t Go_Num_Re = 0;

void Remake_Path()
{
    static PATH_BesselControlPoint Remake_Bessel = {0};
    static Pose Remake_StraightPath = {0};
    switch (Go_Num_Re) {
        case 0:
            if(Robot_Pose.wx != 0 && Robot_Pose.wy != 0)
                Go_Num_Re++;
            break;
        case 1:
            Remake_Bessel.Path_BesselPoint_1 = Robot_Pose;
            Remake_Bessel.Path_BesselPoint_2.x = 6100.39f;
            Remake_Bessel.Path_BesselPoint_2.y = -540.834f;
            Remake_Bessel.Path_BesselPoint_3.x = 6100.f;
            Remake_Bessel.Path_BesselPoint_3.y = 808.7f;
            Remake_Bessel.Path_BesselPoint_4.wx = 6100.f;
            Remake_Bessel.Path_BesselPoint_4.wy = 3824.6f;
            Remake_Bessel.Path_BesselPoint_4.yaw = Robot_Pose.yaw;
            Go_Num_Re++;
            break;
        case 2:
            if(Path_BesselPathInit(&Remake_Bessel,false,700,700,300,5,30,10))
                Go_Num_Re++;
            break;
        case 3:
            if(goPath() == FINISH)
                Go_Num_Re++;
            break;
        case 4:
            Remake_StraightPath.wx = 7908.89f;
            Remake_StraightPath.wy = 3650.79f;
            Remake_StraightPath.yaw = Robot_Pose.yaw;

            Remake_Bessel.Path_BesselPoint_1 = Robot_Pose;
            Remake_Bessel.Path_BesselPoint_2.x = (Remake_StraightPath.wx - Robot_Pose.wx) * 0.3f + Robot_Pose.wx;
            Remake_Bessel.Path_BesselPoint_2.y = (Remake_StraightPath.wy - Robot_Pose.wy) * 0.3f + Robot_Pose.wy;
            Remake_Bessel.Path_BesselPoint_3.x = (Remake_StraightPath.wx - Robot_Pose.wx) * 0.6f + Robot_Pose.wx;
            Remake_Bessel.Path_BesselPoint_3.y = (Remake_StraightPath.wy - Robot_Pose.wy) * 0.6f + Robot_Pose.wy;
            Remake_Bessel.Path_BesselPoint_4 = Remake_StraightPath;
            Go_Num_Re++;
            break;
        case 5:
            if(Path_BesselPathInit(&Remake_Bessel,false,700,700,500,10,40,10))
                Go_Num_Re++;
            break;
        case 6:
            if(goPath() == FINISH)
            {
                Go_Num_Re++;
                setPidCalMode(PID_OFF);
            }
            break;

    }

}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
    if(huart->Instance == USART2)
    {
        /******************USER**********************/
        //数据包长度
        static uint8_t Rx_len = 14;
        //包头
        static uint8_t Head = 0x77;
        /********************************************/
        static uint8_t temp = 0;
        static bool flag_cal_sum = false;
        static uint8_t Num =0;
        if(temp == Head && flag_cal_sum == false)
        {
            flag_cal_sum = true;
            Num = 0;
        }
        if(flag_cal_sum == true)
        {
            if(Num>0 && Num<Rx_len-1)
                gyro_sum += temp;
            gyro.Gyro_Data[Num] = temp;
            Num++;
            if(Num>=Rx_len-1) {
                flag_cal_sum = false;
                gyro_sum = 0;
            }
        }
        HAL_UART_Receive_IT(&huart2,&temp,1);
    }


}



uint8_t RxBuff1[8];
uint8_t RxBuff2[8];
CAN_RxHeaderTypeDef RxHeader1;
CAN_RxHeaderTypeDef RxHeader2;


void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    if(hcan->Instance == CAN1)
    {
        if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader1, RxBuff1)==HAL_OK) {
            __HAL_CAN_ENABLE_IT(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
        }
    }
//    if(hcan->Instance == CAN2)
//    {
//        if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader2, RxBuff2)==HAL_OK) {
//            __HAL_CAN_ENABLE_IT(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);
//        }
//    }


}
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)    //ADC转换完成回调
{DT35_ADCCallback(hadc);

}